#include "hb_msgs/msg/heart_beat.hpp"
#include "laser_controller.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"

class SenkyLaser_node : public rclcpp::Node
{
  private:
	int send_id, recv_id;
	std::string group_ip, local_ip, remote_ip;
	int port;
	// modbus communication controller class
	std::shared_ptr<laser_controller> controller;
	// used to publish sample result(in mm, valid when >0)
	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher;

	rclcpp::TimerBase::SharedPtr hb_timer;
	rclcpp::Publisher<hb_msgs::msg::HeartBeat>::SharedPtr hb_pub;
	int error_code;
	int sample_cnt = 0;

	// if this host is bypassed
	bool bypassed = false;
	std::thread listen_thread;

	/**
	 * @brief enable or disable bypass mode. in bypass mode, we release rs485 bus
	 *
	 * @param enable true: enable, false: disable
	 * @return int 0: success, <0: failed
	 */
	int enable_bypass(bool enable)
	{
		this->bypassed = enable;
		return 0;
	}

	void listen_func(void)
	{
		int err;
		std_msgs::msg::Float64 msg;
		int status, distance;
		while (rclcpp::ok())
		{
			do
			{
				err = this->controller->read_frame(status, distance);
				switch (err)
				{
					case -1: // socket error, timeout or others
						this->error_code = -1;
						break;
					case -2: // incorrect dlc
						this->error_code = -2;
						break;
					case 0: // unconcerned
						break;
					default: // valid frame
						if (status == 0 && !this->bypassed)
						{
							// for whatever reason, laser not enabled
							RCLCPP_INFO(this->get_logger(), "laser shut down, try restart");
							this->controller->write_frame(true);
						}
						this->error_code = 0;
						msg.data = distance > 0 ? distance * 0.1 : -20000;
						this->sample_cnt += 1;
						this->publisher->publish(msg);
						break;
				}
			} while (0);
			usleep(100);
		}
		RCLCPP_ERROR(this->get_logger(), "listen thread exit");
	}

	void hb_callback(void)
	{
		hb_msgs::msg::HeartBeat hb;
		hb.name = this->get_fully_qualified_name();
		hb.stamp = rclcpp::Clock().now();

		switch (this->error_code)
		{
			case -1:
				hb.code = -1;
				hb.reason = "socket read return -1";
				this->error_code = 0;
				break;
			case -2:
				hb.code = -2;
				hb.reason = "incorrect dlc";
				break;
			default:
				hb.code = 0;
				hb.reason = "ok";
				break;
		}

		if (hb.code == 0)
		{
			if (this->sample_cnt == 0)
			{
				hb.code = -3;
				hb.reason = "no frame from device";
			}
		}
		sample_cnt = 0;
		this->hb_pub->publish(hb);
	}

  public:
	int success = 0;
	SenkyLaser_node(std::string name) : Node(name)
	{
		RCLCPP_INFO(this->get_logger(), "laser node started");
		// decleare parameters
		this->declare_parameter<int>("send_id", 646);
		this->declare_parameter<int>("recv_id", 774);
		this->declare_parameter<std::string>("group_ip", "239.255.0.6");
		this->declare_parameter<int>("port", 6781);
		this->declare_parameter<std::string>("remote_ip", "192.168.1.100");
		this->declare_parameter<std::string>("local_ip", "192.168.1.201");
		this->declare_parameter<bool>("bypassed", bypassed);
		this->get_parameter("send_id", send_id);
		this->get_parameter("group_ip", group_ip);
		this->get_parameter("local_ip", local_ip);
		this->get_parameter("remote_ip", remote_ip);
		this->get_parameter("port", port);
		this->get_parameter("recv_id", recv_id);
		this->get_parameter("bypassed", bypassed);
		RCLCPP_INFO(this->get_logger(),
					"in multicast group %s, port %d, remote host %s, local host %s, send id "
					"%d, recv id %d",
					this->group_ip.c_str(), this->port, this->remote_ip.c_str(),
					this->local_ip.c_str(), this->send_id, this->recv_id);
		if (this->bypassed)
		{
			RCLCPP_INFO(this->get_logger(), "NOTE: this host is bypassed by launch parameter");
		}

		this->controller = std::make_shared<laser_controller>(
			std::make_shared<iolayer::layer_socket>(group_ip, port, remote_ip, local_ip),
			send_id, recv_id);

		this->publisher = this->create_publisher<std_msgs::msg::Float64>("laser_distance", 10);
		this->listen_thread = std::thread(std::bind(&SenkyLaser_node::listen_func, this));
		this->listen_thread.detach();
		// create publisher to publish distance data(in mm)
		RCLCPP_INFO(this->get_logger(), "publishing distance data(std_msgs/msg/Float64) to "
										"topic 'laser_distance' periodically...");

		this->hb_pub = this->create_publisher<hb_msgs::msg::HeartBeat>("/heartbeat", 1);
		this->hb_timer = this->create_wall_timer(
			std::chrono::milliseconds(200), std::bind(&SenkyLaser_node::hb_callback, this));

		success = 1;
	}

	~SenkyLaser_node()
	{
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<SenkyLaser_node>("laser_node");
	if (node->success) rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
